//
//  test markerAR.cpp
//  markerARLib
//
//  Created by fg on 5/18/16.
//  Copyright © 2016 CvidKal. All rights reserved.
//

#include <stdio.h>
#include "globalInit.h"
#include "global.h"

#include <fstream>
#include <opencv2/opencv.hpp>
using namespace markerAR;
using namespace cv;
#include "Marker.h"
#include <ceres/rotation.h>
#include "Algorithm.hpp"
#include <Eigen/Dense>
#include "glog/logging.h"
#include <GLUT/GLUT.h>
#include "BoWWrapper.hpp"
#include "TestImage.hpp"
#include <chrono>
using namespace google;



void addMarker()
{
    markerAR::ORB orb;

    cv::Mat img = imread("/Users/fg/Desktop/marker1.jpg",0);
    if (img.size().height == 0) {
        LOG(FATAL)<<"Empty Image"<<endl;
    }
    std::ifstream fin("/Users/fg/Desktop/0620-103644/IMU_0001.txt");
    double IMU[4];
    fin>>IMU[0]>>IMU[1]>>IMU[2]>>IMU[3];
    fin.close();
    
    std::vector<KeyPoint>kpts;
    cv::Mat desp;
    orb(img,Mat(),kpts,desp);
    bow.addImage(kpts, desp);
    
    Eigen::Matrix<double, 3, 3,Eigen::RowMajor> R;
    ceres::QuaternionToRotation(IMU, R.data());
    //        ceres::EulerAnglesToRotationMatrix(IMU, 3, R.data());
    auto m = new Marker(img,R,0);
    markerAR::map->addMarker(m);
    img = imread("/Users/fg/Desktop/marker2.jpg",0);
    fin.open("/Users/fg/Desktop/marker2.txt");
    fin>>IMU[0]>>IMU[1]>>IMU[2]>>IMU[3];
    fin.close();
    auto m2 = new Marker(img,R,1);
    markerAR::map->addMarker(m2);
    orb(img,Mat(),kpts,desp);
    bow.addImage(kpts, desp);
    bow.train();
}

void addMarker2()
{
       Mat m1(360,480,CV_8UC3);
        Mat m2 = m1.clone();
    Mat m3 = m2.clone();
    Mat m4 = m3.clone();
        memcpy(m1.data, marker1, sizeof(marker1));
    
        memcpy(m2.data, marker2, sizeof(marker2));
    
    memcpy(m3.data, marker3, sizeof(marker3));
    
    memcpy(m4.data, marker4, sizeof(marker4));
    std::vector<KeyPoint>kpts;
    cv::Mat desp;
    orb->operator()(m1,Mat(),kpts,desp);
    bow.addImage(kpts, desp);
    auto R = Solve3D::setRotationMatrix(0.0, 0.0, 0.0);
    auto m = new Marker(m1,R,0);
    markerAR::map->addMarker(m);
    
    orb->operator()(m2,Mat(),kpts,desp);
    bow.addImage(kpts, desp);
     R = Solve3D::setRotationMatrix(0.0, 0.0, 0.0);
     m = new Marker(m2,R,1);
    markerAR::map->addMarker(m);
    
    orb->operator()(m3,Mat(),kpts,desp);
    bow.addImage(kpts, desp);
    R = Solve3D::setRotationMatrix(0.0, 0.0, 0.0);
    m = new Marker(m3,R,1);
    markerAR::map->addMarker(m);
    
    orb->operator()(m4,Mat(),kpts,desp);
    bow.addImage(kpts, desp);
    R = Solve3D::setRotationMatrix(0.0, 0.0, 0.0);
    m = new Marker(m4,R,1);
    markerAR::map->addMarker(m);
    bow.train();
}



int main(int argc,char**argv)
{
    
    google::InitGoogleLogging(argv[0]);
    FLAGS_alsologtostderr = 1;
    fLS::FLAGS_log_dir = "/Users/fg/cycle/log";
    globalInit(640, 480, 520);
    addMarker2();
    Relocalisation relocal;
    Mat img;
    std::chrono::system_clock clock;
    auto start = clock.now();
    for (int i=1; i<200; ++i) {
        char s[100];
        sprintf(s, "/Users/fg/Desktop/0620-103644/Image_%04d.jpg",i);
        img = imread(s);
        resize(img, img, cv::Size(480,360));
//        imshow("now", img);
//        cvWaitKey();
        
        double timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(clock.now().time_since_epoch()-start.time_since_epoch()).count();
        if(track->getState()==Tracking::Not_Initialise)
        {
            initialzer->tryInitialize(img,timestamp);
            //            relocal.tryRelocalisationByBoW(img);
//            std::vector<cv::KeyPoint> kpts;
//            cv::Mat desp;
//            orb->operator()(img, Mat(), kpts,desp);
//            auto result = bow.query(kpts, desp);
//            track->tryAddMarker(img,result);
            continue;
        }
        
        TickMeter tm;
        tm.start();
        auto C = track->getC();
        bool flag =  track->tracking(img,timestamp);
        cout<<tm.getTimeSec()<<endl;
        
    }
    track->OutputTrajectory("/Users/fg/cycle/trajectoryCeres.txt");
    return 0;
}